import cv2
import numpy as np
import math


# import decimal
# from decimal import Decimal


class HUD:
    img = None
    d_img = None
    msg = None
    bt = 0  # 电池
    # 设置HUD信息
    ang = 0  # 偏航角度
    altitude = 0  # 高度
    pitch = 0  # 俯仰角度
    roll = 0  # 滚转角度
    speed = 0  # 速度

    def get_output(self):
        return self.d_img

    def __init__(self):
        pass

    def set_img(self, img):
        self.img = img
        self.msg = img.shape

    def img_deal(self, img):
        self.img = img
        self.msg = img.shape
        self.add_attitude()
        return self.d_img

    def getBt(self):
        return self.bt

    def setBt(self, bt):
        self.bt = bt

    def getAltitude(self):
        return self.altitude

    def setAltitude(self, altitude):
        self.altitude = altitude

    def getPitch(self):
        return self.pitch

    def setPitch(self, pitch):
        self.pitch = pitch

    def getAng(self):
        return self.ang

    def setAng(self, ang):
        self.ang = ang

    def getRoll(self):
        return self.roll

    def setRoll(self, roll):
        self.roll = roll

    def getSpeed(self):
        return self.speed

    def setSpeed(self, speed):
        self.speed = speed

    def add_attitude(self):
        # 创建一个空白图像作为背景

        height = self.msg[0]
        width = self.msg[1]
        bt = self.bt  # 电池
        # 设置HUD信息
        ang = self.ang  # 偏航角度
        altitude = self.altitude  # 高度
        pitch = self.pitch  # 俯仰角度
        roll = self.roll  # 滚转角度
        speed = self.speed  # 速度
        image = np.zeros((height, width, 3), dtype=np.uint8)
        # 文本
        battery_percentage = 1 - 0.01 * bt
        altitudeText = "Altitude"
        speedText = "Speed"

        # 设置绘制线条和弧线的颜色和粗细
        line_color = (0, 255, 0)  # 绿色
        line_thickness = 1
        # 绘制文本信息
        font = cv2.FONT_HERSHEY_SIMPLEX
        font_scale = 1
        font_color = (0, 255, 0)  # 绿色
        text_margin = 30
        battery_height = width * 0.035
        bw = width * 0.95
        bh = height * 0.05
        center = (int(width / 2), int(height / 2))
        radius = int(min(height, width) / 2) - 10
        signFont = cv2.FONT_HERSHEY_TRIPLEX
        sign_line_thickness = 2
        topFont = cv2.FONT_HERSHEY_COMPLEX
        # 绘制刻度#偏航角度表盘

        for angle in range(ang - 45, ang + 45):
            angle_rad = math.radians(angle)
            start_point = (
                int(center[0] + (radius - 50) * math.cos(angle_rad)),
                int(center[1] - (radius - 50) * math.sin(angle_rad))
            )
            if angle % 10 == 0:
                end_point = (
                    int(center[0] + radius * math.cos(angle_rad)),
                    int(center[1] - radius * math.sin(angle_rad))
                )
                cv2.line(image, start_point, end_point, (0, 255, 0), line_thickness, cv2.LINE_AA)  # 长刻度
            elif angle % 2 == 0:
                end_point = (
                    int(center[0] + (radius - 10) * math.cos(angle_rad)),
                    int(center[1] - (radius - 10) * math.sin(angle_rad))
                )
                cv2.line(image, start_point, end_point, (0, 255, 0), line_thickness, cv2.LINE_AA)  # 短刻度

        # 计算旋转变换矩阵
        M = cv2.getRotationMatrix2D(center, 90 - ang, 1.0)

        # 进行旋转变换
        image = cv2.warpAffine(image, M, (width, height))

        '''分割线
        整合imageZ部分如下
        '''

        imageZ = np.zeros((height, width, 3), dtype=np.uint8)

        """姿态"""
        for angle in range(-360, height + 360):
            if angle % 80 == 0 and height / 4 <= angle + pitch * 8 <= height * 3 / 4:
                start_point = (
                    int(center[0] - 25),
                    int(angle + pitch * 8)
                )
                end_point = (
                    int(center[0] + 25),
                    int(angle + pitch * 8)
                )

                cv2.line(imageZ, start_point, end_point, (0, 255, 0), line_thickness, cv2.LINE_AA)  # 短刻度
            elif angle % 40 == 0 and height / 4 <= angle + pitch * 8 <= height * 3 / 4:
                start_point = (
                    int(center[0] - 35),
                    int(angle + pitch * 8)
                )
                end_point = (
                    int(center[0] + 35),
                    int(angle + pitch * 8)
                )
                cv2.line(imageZ, start_point, end_point, (0, 255, 0), line_thickness, cv2.LINE_AA)
                cv2.putText(imageZ, str(int(abs((1080 - angle) / 8 - 90))), end_point, font, font_scale, font_color,
                            line_thickness,
                            cv2.LINE_AA)
        cv2.line(imageZ, (int(width / 3), int(center[1])), (int(width * 2 / 3), int(center[1])), (0, 255, 0),
                 line_thickness, cv2.LINE_AA)
        M = cv2.getRotationMatrix2D(center, roll, 1.0)
        imageZ = cv2.warpAffine(imageZ, M, (width, height))
        """速度"""
        for angle in range(-150, height + 150):
            if angle % 40 == 0 and height / 4 <= angle + speed * 4 <= height * 3 / 4 and (
                    angle / 4 - height / 4 + 90) <= 0:
                start_point = (
                    0,
                    int(angle + speed * 4)
                )
                end_point = (
                    35,
                    int(angle + speed * 4)
                )
                cv2.line(imageZ, start_point, end_point, (0, 255, 0), line_thickness, cv2.LINE_AA)  # 短刻度
            elif angle % 20 == 0 and height / 4 <= angle + speed * 4 <= height * 3 / 4 and (
                    angle / 4 - height / 4 + 90) <= 0:
                start_point = (
                    0,
                    int(angle + speed * 4)
                )
                end_point = (
                    25,
                    int(angle + speed * 4)
                )
                cv2.line(imageZ, start_point, end_point, (0, 255, 0), line_thickness, cv2.LINE_AA)
                cv2.putText(imageZ, str(int(abs(angle / 4 - height / 4 + 90))), (35, int(angle + speed * 4) + 5), font,
                            font_scale, font_color,
                            line_thickness,
                            cv2.LINE_AA)
        cv2.line(imageZ, (0, int(center[1])), (80, int(center[1])), (255, 51, 255),
                 sign_line_thickness, cv2.LINE_AA)
        cv2.putText(imageZ, str(speed), (80, int(center[1]) + 10), signFont, font_scale, (255, 51, 255), line_thickness,
                    cv2.LINE_AA)
        cv2.putText(imageZ, speedText, (0, int(height / 5)), topFont, font_scale, (0, 255, 0), line_thickness,
                    cv2.LINE_AA)
        """高度"""
        for angle in range(-360, height + 360):
            if angle % 40 == 0 and height / 4 <= angle + altitude * 4 <= height * 3 / 4 and angle / 4 - height / 4 + 90 <= 0:
                start_point = (
                    width,
                    int(angle + altitude * 4)
                )
                end_point = (
                    width - 50,
                    int(angle + altitude * 4)
                )
                cv2.line(imageZ, start_point, end_point, (0, 255, 0), line_thickness, cv2.LINE_AA)  # 短刻度
            elif angle % 20 == 0 and height / 4 <= angle + altitude * 4 <= height * 3 / 4 and angle / 4 - height / 4 + 90 <= 0:
                start_point = (
                    width,
                    int(angle + altitude * 4)
                )
                end_point = (
                    width - 25,
                    int(angle + altitude * 4)
                )
                cv2.putText(imageZ, str(int(abs(angle / 4 - height / 4 + 90))),
                            (width - 80, int(angle + altitude * 4) + 5), font, font_scale, font_color,
                            line_thickness,
                            cv2.LINE_AA)
                cv2.line(imageZ, start_point, end_point, (0, 255, 0), line_thickness, cv2.LINE_AA)  # 短刻度
        cv2.line(imageZ, (width - 60, int(center[1])), (width, int(center[1])), (255, 51, 255),
                 sign_line_thickness, cv2.LINE_AA)
        cv2.putText(imageZ, altitudeText, (width - 135, int(height / 5)), topFont, font_scale, (0, 255, 0),
                    line_thickness, cv2.LINE_AA)
        cv2.putText(imageZ, str(altitude), (width - 120, int(center[1]) + 10), signFont, font_scale, (255, 51, 255),
                    line_thickness,
                    cv2.LINE_AA)
        '''分割线
        整合imageZ部分如上
        '''
        # 文字数显

        cv2.putText(image, str(ang), (center[0], 85), font, font_scale, font_color, line_thickness, cv2.LINE_AA)  # 角度
        cv2.line(image, (center[0], 0), (center[0], 85), (0, 255, 0), line_thickness, cv2.LINE_AA)
        # 电池图像显示
        cv2.rectangle(image, (round(bw), round(bh)), (round(bw + width * 0.02), round(bh + battery_height)),
                      (192, 192, 192), -round(width * 0.02), cv2.LINE_AA)
        cv2.rectangle(image, (round(bw), round(bh + battery_percentage * battery_height)),
                      (round(bw + width * 0.02), round(bh + battery_height)), (124, 252, 0), -round(width * 0.02),
                      cv2.LINE_AA)
        ds = cv2.addWeighted(imageZ, 1, image, 1, 0)
        self.d_img = cv2.addWeighted(self.img, 1, ds, 1, 0)